/**
* @file ImuData.h
* @author
* @brief IMU数据类型
* @version 1.0
* @date 2020-07-23
*
* @copyright Copyright (c) 2020
*
*/

#pragma once

#include <cstdint>
#include <Eigen/Eigen>
#include <cmath>
#include "tf2/utils.h"

namespace behavior_controller
{
class ImuData
{
public:
    struct ImuInRobot
    {
        Eigen::Vector3d position;
        Eigen::Quaterniond quaternion;
    };

    ImuData(){};

    ImuData(double &vel_x, double &vel_y, double &vel_z,
            double &acc_x, double &acc_y, double &acc_z,
            double &roll, double &pitch, double &yaw)
            : roll_(roll), pitch_(pitch), yaw_(yaw),
              vel_x_(vel_x), vel_y_(vel_y), vel_z_(vel_z),
              acc_x_(acc_x), acc_y_(acc_y), acc_z_(acc_z){};

    ImuData(double &vel_x, double &vel_y, double &vel_z,
            double &acc_x, double &acc_y, double &acc_z,
            double x, double y, double z, double w)
            : vel_x_(vel_x), vel_y_(vel_y), vel_z_(vel_z),
              acc_x_(acc_x), acc_y_(acc_y), acc_z_(acc_z)
    {
        set_rpy(x, y, z, w);
        SetTime();
    };

    void updateData(double &vel_x, double &vel_y, double &vel_z,
                    double &acc_x, double &acc_y, double &acc_z,
                    double x, double y, double z, double w)
    {
        vel_x_ = vel_x;
        vel_y_ = vel_y;
        vel_z_ = vel_z;
        acc_x_ = acc_x;
        acc_y_ = acc_y;
        acc_z_ = acc_z;
        set_rpy(x, y, z, w);
        SetTime();

    };
    /**
     * @brief 获取roll的角度
     * @return
     */

    void set_rpy(double &roll, double &pitch, double &yaw)
    {
        roll_ = roll;
        pitch_ = pitch;
        yaw_ = yaw;
    }

    void set_rpy(double &x, double &y, double &z, double &w)  // imu消息中的quaternion 是大地坐标系下（target frame）测得的imu硬件的朝向
    {
        //tf2::Quaternion quat1 = tf2::Quaternion(x, y, z, w);
        //tf2::Matrix3x3(quat1).getRPY(roll_, pitch_, yaw_);//
        //std::cout << "1 roll_= " << roll_<< "  pitch_= " << pitch_<<"  yaw_= " << yaw_<<  std::endl;

        //auto q= Eigen::Quaterniond(w,x,y,z)*imu_in_robot.quaternion; // imu消息中的quaternion 是大地坐标系下（target frame）测得的imu硬件的朝向
        //tf2::Quaternion quat2 = tf2::Quaternion(q.x(), q.y(), q.z(),q.w());
        //tf2::Matrix3x3(quat2).getRPY(roll_, pitch_, yaw_);//
        //std::cout << "2 roll_= " << roll_<< "  pitch_= " << pitch_<<"  yaw_= " << yaw_<<  std::endl;

        auto robot_in_imu = imu_in_robot.quaternion.inverse();
        auto q2 = Eigen::Quaterniond(w, x, y, z) * robot_in_imu; //得到小车坐标系相对于大地坐标系方向
        tf2::Quaternion quat3 = tf2::Quaternion(q2.x(), q2.y(), q2.z(), q2.w());
        tf2::Matrix3x3(quat3).getRPY(roll_, pitch_, yaw_);//
        //std::cout << "3 roll_= " << roll_<< "  pitch_= " << pitch_<<"  yaw_= " << yaw_<<  std::endl;
    }

    double max_abs_tilt() const
    {
        return sqrt(pow(roll_, 2) + pow(pitch_, 2));
    }


    double roll() const { return roll_; }

    /**
     * @brief 获取pitch的角度
     * @return
     */
    double pitch() const { return pitch_; }

    /**
     * @brief 获取yaw的角度
     * @return
     */
    double yaw() const { return yaw_; }

    /**
     * @brief 获取围绕x轴的角速度
     * @return
     */
    double vel_x() const { return vel_x_; }

    /**
     * @brief 获取围绕y轴的角速度
     * @return
     */
    double vel_y() const { return vel_y_; }

    /**
     * @brief 获取围绕z轴的角速度
     * @return
     */
    double vel_z() const { return vel_z_; }

    void set_imu_pose(ImuInRobot imu_in_robot_)
    { imu_in_robot = imu_in_robot_; };

    ImuInRobot get_imu_pose() { return imu_in_robot; };

    /**
     * @brief 获取x轴的加速度
     * @return
     */
    double acc_x() const { return acc_x_; }

    /**
    * @brief 返回y轴的加速度
    * @return
    */
    double acc_y() const { return acc_y_; }

    /**
    * @brief 返回z轴的加速度
    * @return
    */
    double acc_z() const { return acc_z_; }

    void SetTime() { real_time = Time::Now(); };

    void SetTime(double _time) { real_time = _time; };

    double GetRealTime() { return real_time; };

    bool SetFrame(std::string imu_frame_)
    {
        imu_frame = imu_frame_;
        return true;
    };

    std::string GetFrame() { return imu_frame; };

private:
    ImuInRobot imu_in_robot;
    std::string imu_frame;
    double real_time = 0;
    double roll_;  // 俯仰角
    double pitch_; // 翻滚角
    double yaw_;   // 偏航角
    double vel_x_; // x轴角速度
    double vel_y_; // y轴角速度
    double vel_z_; // z轴角速度
    double acc_x_; // x轴角加速度
    double acc_y_; // y轴角加速度
    double acc_z_; // z轴角加速度
};
} // namespace behavior_controller
